#include "../inc/motor.h"
#include "../inc/color.h"
#include <avr/io.h>

volatile int speed_left=11;	//In case that we set speed 0 at the first time.
volatile int speed_right=11;	//In case that we set speed 0 at the first time.

void set_speed(char wheel, int speed)
{
	if(wheel==LEFT_WHEEL)
	{
		if(speed==speed_left)
			return;
		speed_left=speed;

		if(speed>=0)
		{	
			clear_bit(PORTC, 6);
			OCR1A=0xFFFF-7999+speed*COEF_LEFT;
		}
		else
		{
			set_bit(PORTC, 6);
			OCR1A=0xFFFF+1+speed*COEF_LEFT;
		}
	}
	else
	{
		if(speed==speed_right)
			return;
		speed_right=speed;
		if(speed>=0)
		{	
			clear_bit(PORTC, 4);
			OCR1B=0xFFFF-7999+speed*COEF_RIGHT;
		}
		else
		{
			set_bit(PORTC, 4);
			OCR1B=0xFFFF+1+speed*COEF_RIGHT;
		}
	}
}

void go_white_line(int direction)
{
	char color_0,color_2;
	while((color_0=get_color(0))!=(color_2=get_color(2)))
	{
		if(color_0==WHITE)
			set_speed(LEFT_WHEEL*direction,7*direction);
		if(color_2==WHITE)
			set_speed(RIGHT_WHEEL*direction,7*direction);
	}
}

void turn(char side, char direction)
{
	set_speed(LEFT_WHEEL, side*direction*9);
	set_speed(RIGHT_WHEEL, -side*direction*9);
}

void enter_gap()
{
	while (1)
	{
		go_white_line(FORWARD);
		
		if(get_color(3)==WHITE)
		{
			set_speed(LEFT_WHEEL,0);
			set_speed(RIGHT_WHEEL,0);
			break;
		}
	}
}
